#define _POSIX_C_SOURCE 200112L
 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <math.h>
#include <limits.h>
#include <errno.h>
#include <libgen.h>
#include <limits.h>
#include <sched.h>
#include <sys/resource.h>
#include <sys/mman.h>
#include <pthread.h>
#include "common.h"

uint64_t get_monotonic_time_ns(void) {
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return (uint64_t)ts.tv_sec * 1000000000ULL + (uint64_t)ts.tv_nsec;
}


void init_periodic_timer(periodic_timer_t *timer, uint64_t period_ns){
    timer->period_ns = period_ns;
    timer->base_period_ns = period_ns;
    timer->max_response_ns = 0;
    timer->adjust_count = 0;
    timer->last_adjust_time = 0;
    timer->need_reset_stats = false;
    timer->skip_next_interval = false;

    clock_gettime(CLOCK_MONOTONIC, &(timer->next_period));
    // 确保第一次等待完整周期
    timer->next_period.tv_nsec += period_ns;
    while (timer->next_period.tv_nsec >= NS_PER_SEC) {
        timer->next_period.tv_sec++;
        timer->next_period.tv_nsec -= NS_PER_SEC;
    }
}

void wait_next_period(periodic_timer_t *timer) {
    uint64_t current_time = get_monotonic_time_ns();
    
    // 固定1ms周期，不进行调整
    timer->period_ns = 1000000ULL; // 固定为1ms
    
    // 计算下一个周期时间点
    timer->next_period.tv_nsec += timer->period_ns;
    while (timer->next_period.tv_nsec >= NS_PER_SEC) {
        timer->next_period.tv_sec++;
        timer->next_period.tv_nsec -= NS_PER_SEC;
    }

    uint64_t target_ns = timer->next_period.tv_sec * NS_PER_SEC + timer->next_period.tv_nsec;
    
    if (current_time < target_ns) {
        int ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &timer->next_period, NULL);
        
        if (ret != 0) {
            if (ret == EINTR) {
                printf("睡眠被信号中断\n");
            } else {
                printf("clock_nanosleep错误: %d\n", ret);
            }
        }
    } else {
        // // 记录错过deadline的情况
        // uint64_t missed_by = current_time - target_ns;
        // // printf("Deadline missed by %lld ns\n", (long long)missed_by);
    }
}



void update_latency_stats(latency_stats_t *stats, int64_t latency_ns) {
    // 更新最小延迟
    if (latency_ns < stats->min_latency) {
        stats->min_latency = latency_ns;
    }
    
    // 更新最大延迟
    if (latency_ns > stats->max_latency) {
        stats->max_latency = latency_ns;
    }
    
    // 累加总延迟
    stats->total_latency += latency_ns;
    
    // 更新直方图
    int64_t latency_us = latency_ns / 1000;
    if (latency_us >= 0 && latency_us < MAX_HISTOGRAM_BINS) {
        stats->histogram[latency_us]++;
    } else {
        stats->over_histogram++;
    }
}

void print_latency_stats(const latency_stats_t *stats) {
    printf("\n=== 延迟统计 ===\n");
    printf("最小延迟: %.3f us\n", stats->min_latency / 1000.0);
    printf("最大延迟: %.3f us\n", stats->max_latency / 1000.0);
    printf("平均延迟: %.3f us\n", 
           (stats->total_latency / (stats->over_histogram + 1)) / 1000.0);
    
    printf("\n延迟分布直方图(us):\n");
    for (int i = 0; i < MAX_HISTOGRAM_BINS; i++) {
        if (stats->histogram[i] > 0) {
            printf("%4d-%4d: %lu\n", i, i+1, stats->histogram[i]);
        }
    }
    
    if (stats->over_histogram > 0) {
        printf("超过%dus的延迟计数: %lu\n", MAX_HISTOGRAM_BINS, stats->over_histogram);
    }
}


void init_timing_stats(timing_stats_t *stats) {
    memset(stats, 0, sizeof(timing_stats_t));
    stats->response.min_response = INT64_MAX;
    // stats->response.max_response = 0;
    stats->interval.min_interval = INT64_MAX;
    // stats->interval.max_interval = 0;
    stats->jitter.min_jitter = INT64_MAX;
    // stats->jitter.max_jitter = 0;

        // 直方图初始化为0
    memset(stats->response_histogram, 0, sizeof(stats->response_histogram));
    memset(stats->interval_histogram, 0, sizeof(stats->interval_histogram));
    stats->over_histogram = 0;
}

// 更新统计
void update_timing_stats(timing_stats_t *stats, int64_t response_time, int64_t interval_time,
                        int64_t interval_jitter,bool reset_stats) {

    // // 忽略不合理的间隔时间（小于100μs）
    // if (interval_time < 100000) { // 100μs
    //     return;
    // }

    if (reset_stats) {
        // 重置统计
        memset(stats, 0, sizeof(timing_stats_t));
        stats->response.min_response = INT64_MAX;
        stats->interval.min_interval = INT64_MAX;
        stats->jitter.min_jitter = INT64_MAX;
        init_cycle_stats(stats);
    }
    // 更新响应时间统计
    if (response_time < stats->response.min_response) {
        stats->response.min_response = response_time;
    }
    if (response_time > stats->response.max_response) {
        stats->response.max_response = response_time;
    }
    stats->response.total_response += response_time;
    stats->response.response_samples++;
    
    // 总是统计间隔和抖动（固定周期）
    if (interval_time < stats->interval.min_interval) {
        stats->interval.min_interval = interval_time;
    }
    if (interval_time > stats->interval.max_interval) {
        stats->interval.max_interval = interval_time;
    }
    stats->interval.total_interval += interval_time;
    stats->interval.interval_samples++;
    
    // 更新周期抖动统计
    int64_t abs_jitter = llabs(interval_jitter);
    if (abs_jitter < stats->jitter.min_jitter) {
        stats->jitter.min_jitter = abs_jitter;
    }
    if (abs_jitter > stats->jitter.max_jitter) {
        stats->jitter.max_jitter = abs_jitter;
    }
    stats->jitter.total_jitter += abs_jitter;
    stats->jitter.jitter_samples++;
    
    // 更新响应时间直方图(us)
    int64_t response_us = response_time / 1000;
    if (response_us >= 0 && response_us < MAX_HISTOGRAM_BINS) {
        stats->response_histogram[response_us]++;
    } else {
        stats->over_histogram++;
    }
    
    // 更新间隔时间直方图(us)
    int64_t interval_us = interval_time / 1000;
    if (interval_us >= 0 && interval_us < MAX_HISTOGRAM_BINS) {
        stats->interval_histogram[interval_us]++;
    }
}

// 计算百分位数
static int64_t calculate_percentile(const uint64_t *histogram, uint64_t total, double percentile) {
    uint64_t count = 0;
    uint64_t target = (uint64_t)(total * percentile);
    
    for (int i = 0; i < MAX_HISTOGRAM_BINS; i++) {
        count += histogram[i];
        if (count >= target) {
            return i * 1000; // 返回纳秒值
        }
    }
    return MAX_HISTOGRAM_BINS * 1000;
}

// 打印统计信息
void print_timing_stats(const timing_stats_t *stats, uint64_t current_period_ns) {
    printf("\n=== 时间统计 ===\n");
    
    // 响应时间统计
    printf("响应时间(us):\n");
    printf("  最小值: %.3f\n", stats->response.min_response / 1000.0);
    printf("  最大值: %.3f\n", stats->response.max_response / 1000.0);
    printf("  平均值: %.3f\n", 
           (stats->response.total_response / (double)stats->response.response_samples) / 1000.0);
    
    // 计算百分位数
    if (stats->response.response_samples > 0) {
        printf("  P50: %.3f\n", 
               calculate_percentile(stats->response_histogram, stats->response.response_samples, 0.5) / 1000.0);
        printf("  P95: %.3f\n", 
               calculate_percentile(stats->response_histogram, stats->response.response_samples, 0.95) / 1000.0);
        printf("  P99: %.3f\n", 
               calculate_percentile(stats->response_histogram, stats->response.response_samples, 0.99) / 1000.0);
    }
    
    // 周期间隔统计
    printf("\n周期间隔(us):\n");
    printf("  目标值: %.3f\n", current_period_ns / 1000.0);
    printf("  实际最小值: %.3f\n", stats->interval.min_interval / 1000.0);
    printf("  实际最大值: %.3f\n", stats->interval.max_interval / 1000.0);
    printf("  实际平均值: %.3f\n", 
           (stats->interval.total_interval / (double)stats->interval.interval_samples) / 1000.0);
    
    // 周期抖动统计
    printf("\n周期抖动(实际间隔-目标间隔, us):\n");
    printf("  最小值: %.3f\n", stats->jitter.min_jitter / 1000.0);
    printf("  最大值: %.3f\n", stats->jitter.max_jitter / 1000.0);
    printf("  平均值: %.3f\n", 
           (stats->jitter.total_jitter / (double)stats->jitter.jitter_samples) / 1000.0);

    // // 添加动态周期信息
    // printf("\n动态周期统计:\n");
    // printf("  实际使用周期: %.3f us\n", (stats->interval.total_interval / (double)stats->interval.interval_samples) / 1000.0);
    // printf("  相对于动态周期的抖动:\n");
    // printf("    最小值: %.3f us\n", stats->jitter.min_jitter / 1000.0);
    // printf("    最大值: %.3f us\n", stats->jitter.max_jitter / 1000.0);
    // printf("    平均值: %.3f us\n", (stats->jitter.total_jitter / (double)stats->jitter.jitter_samples) / 1000.0);
    
    printf("========================\n");
}


// 初始化周期统计
void init_cycle_stats(timing_stats_t *stats) {
    stats->cycle_stats.min_cycle_time = INT64_MAX;
    stats->cycle_stats.max_cycle_time = 0;
    stats->cycle_stats.total_cycle_time = 0;
    stats->cycle_stats.cycle_samples = 0;
    memset(stats->cycle_stats.cycle_histogram, 0, sizeof(stats->cycle_stats.cycle_histogram));
}

// 更新周期统计
void update_cycle_stats(timing_stats_t *stats, int64_t actual_cycle_time) {
    // 更新最小周期时间
    if (actual_cycle_time < stats->cycle_stats.min_cycle_time) {
        stats->cycle_stats.min_cycle_time = actual_cycle_time;
    }
    if (actual_cycle_time > stats->cycle_stats.max_cycle_time) {
        stats->cycle_stats.max_cycle_time = actual_cycle_time;
    }
    
    stats->cycle_stats.total_cycle_time += actual_cycle_time;
    stats->cycle_stats.cycle_samples++;
    
    // 更新周期直方图(us)
    int64_t cycle_us = actual_cycle_time / 1000;
    if (cycle_us >= 0 && cycle_us < MAX_HISTOGRAM_BINS) {
        stats->cycle_stats.cycle_histogram[cycle_us]++;
    }
}

// 计算响应耗时占比
double calculate_response_time_ratio(const timing_stats_t *stats) {
    if (stats->response.response_samples == 0) return 0.0;
    
    // 平均响应时间 / (平均响应时间 + 平均帧间距)
    double avg_response = (double)stats->response.total_response / stats->response.response_samples;
    double avg_interval = (double)stats->interval.total_interval / stats->interval.interval_samples;
    return avg_response / (avg_response + avg_interval);
}

// 打印周期统计
void print_cycle_stats(const timing_stats_t *stats) {
    printf("\n=== 时钟周期精度统计 ===\n");
    printf("目标周期: 1.000 ms\n");
    printf("实际周期范围: %.3f - %.3f ms\n",
           stats->cycle_stats.min_cycle_time / 1000000.0,
           stats->cycle_stats.max_cycle_time / 1000000.0);
    printf("周期抖动: %.3f ms\n",
           ((double)stats->cycle_stats.max_cycle_time - stats->cycle_stats.min_cycle_time) / 1000000.0);
    
    if (stats->cycle_stats.cycle_samples > 0) {
        printf("平均周期: %.3f ms\n",
               (stats->cycle_stats.total_cycle_time / (double)stats->cycle_stats.cycle_samples) / 1000000.0);
    
    // 计算周期分布的百分位数
    printf("周期分布(ms):\n");
    printf("  P50: %.3f\n", calculate_percentile(stats->cycle_stats.cycle_histogram, 
           stats->cycle_stats.cycle_samples, 0.5) / 1000000.0);
    printf("  P95: %.3f\n", calculate_percentile(stats->cycle_stats.cycle_histogram,
           stats->cycle_stats.cycle_samples, 0.95) / 1000000.0);
    printf("  P99: %.3f\n", calculate_percentile(stats->cycle_stats.cycle_histogram,
           stats->cycle_stats.cycle_samples, 0.99) / 1000000.0);
    }
}